#ifndef VOL_HPP
#define VOL_HPP
#include <iostream>
#include <fstream>
#include <mytl/vec.hpp>

using std::size_t;
using mytl::vec;

class norm_vol {
  protected:
  size_t dim_x_, dim_y_, dim_z_;
  public:
	norm_vol() {}
	virtual double operator()(double x, double y, double z) const=0;
	virtual vec<3,double> grad(double x, double y, double z) const=0;
	virtual double operator()(int x, int y, int z) const=0;
	virtual void load(const char * filename)=0;
	virtual ~norm_vol() {};
	virtual int dim_x() const { return dim_x_; }
	virtual int dim_y() const { return dim_y_; }
	virtual int dim_z() const { return dim_z_; }
};

template <class voxel_type>
class vol : public norm_vol {
	voxel_type * data_;
	public:
	vol(size_t _dim_x, size_t _dim_y, size_t _dim_z) {
	  dim_x_=_dim_x;
	  dim_y_=_dim_y;
	  dim_z_=_dim_z;
		data_=new voxel_type[dim_x_*dim_y_*dim_z_];
	}
	virtual double operator()(double x, double y, double z) const {
	  x=(dim_x_-1)*x;
	  y=(dim_y_-1)*y;
	  z=(dim_z_-1)*z;
		int ix=x;
		int iy=y;
		int iz=z;
		double dx=x-ix;
		double dy=y-iy;
		double dz=z-iz;
	  return (*this)(ix,iy,iz)*(1.0-dx)*(1.0-dy)*(1.0-dz)+
	         (*this)(ix+1,iy,iz)*(dx)*(1.0-dy)*(1.0-dz)+
	         (*this)(ix,iy+1,iz)*(1.0-dx)*(dy)*(1.0-dz)+
	         (*this)(ix+1,iy+1,iz)*(dx)*(dy)*(1.0-dz)+
	         (*this)(ix,iy,iz+1)*(1.0-dx)*(1.0-dy)*(dz)+
	         (*this)(ix+1,iy,iz+1)*(dx)*(1.0-dy)*(dz)+
	         (*this)(ix,iy+1,iz+1)*(1.0-dx)*(dy)*(dz)+
	         (*this)(ix+1,iy+1,iz+1)*(dx)*(dy)*(dz);
	}
	virtual vec<3,double> grad(double x, double y, double z) const {
	  double eps=0.0000001;
	  vec<3,double> r;
		double f=(*this)(x,y,z);
		r[0]=((*this)(x+eps,y,z)-f)/eps;
		r[1]=((*this)(x,y+eps,z)-f)/eps;
		r[2]=((*this)(x,y,z+eps)-f)/eps;
		return r;
	}
	double operator()(int x, int y, int z) const {
	  return static_cast<double>(data_[dim_z_*dim_y_*z+dim_y_*y+x]);
	}
	virtual void load(const char * filename) {
    std::ifstream is;
    is.open (filename, std::ios::binary );

    is.read ((char *)data_,dim_x_*dim_y_*dim_z_);
    is.close();
	}
	virtual ~vol() {
	  delete [] data_;
	}
};

class tricubic_vol : public norm_vol {
	float * data_;
//tri-cubic coefficients measurement
  static const float Pole=-0.26794919243112270647255365849413;  //pole for cubic b-spline
  float InitialCausalCoefficient(float* c,			// coefficients
	  size_t DataLength)	// number of coefficients
  {
  	size_t Horizon;
  	if(DataLength<28) 
	  	Horizon=DataLength;
	  else 
	  	Horizon=28;
  	// this initialization corresponds to mirror boundaries
	  // accelerated loop
	  float zn = Pole;
	  float Sum = c[0];
	  for (size_t n = 1; n < Horizon; n++) {
	  	Sum += zn * c[n];
	  	zn *= Pole;
  	}
  	return(Sum);
  }
  float InitialAntiCausalCoefficient(float* c,			// coefficients
  	size_t DataLength)	// number of samples or coefficients
  {
  	// this initialization corresponds to mirror boundaries
  	return((Pole / (Pole * Pole - 1.0f)) * (Pole * c[DataLength - 2] + c[DataLength - 1]));
  }
  void ConvertToInterpolationCoefficients(float* c,			// input samples --> output coefficients
	  size_t DataLength)	// number of samples or coefficients
  {
	  // compute the overall gain
	  const float Lambda = (1.0f - Pole) * (1.0f - 1.0f / Pole);
  	// causal initialization
  	c[0] = Lambda * InitialCausalCoefficient(c, DataLength);
  	// causal recursion
	  for (size_t n = 1; n < DataLength; n++) {
		  c[n] = Lambda * c[n] + Pole * c[n - 1];
	  }
	  // anticausal initialization
	  c[DataLength - 1] = InitialAntiCausalCoefficient(c, DataLength);
	  // anticausal recursion
	  for (int n = DataLength - 2; 0 <= n; n--) {
		  c[n] = Pole * (c[n + 1] - c[n]);
  	}
  }
  float bspline(float t) const
  {
  	t = abs(t);
  	float a = 2.0f - t;

	  if (t < 1.0f) 
  		return 2.0f/3.0f - 0.5f*t*t*a;
  	else if (t < 2.0f) 
  		return a*a*a / 6.0f;
  	else 
  		return 0.0f;
  }

  float bsplined(float t) const
  {
  	float c=t<0 ? -1.0f : 1.0f;
  	t = abs(t);
  	float a = 2.0f - t;

	  if (t < 1.0f) 
	  	return c*t*(3.0f*t-4.0f) / 2.0f;
	  else if (t < 2.0f) 
	  	return -c*a*a / 2.0f;
	  else 
		  return 0.0f;
  }


	public:
	explicit tricubic_vol(const norm_vol& v) { 
	  dim_x_=v.dim_x();
	  dim_y_=v.dim_y();
	  dim_z_=v.dim_z();
		data_=new float[dim_x_*dim_y_*dim_z_];
		for(int k=0;k<dim_z();++k) {
		  for(int j=0;j<dim_y();++j) {
		    for(int i=0;i<dim_x();++i) 
          data_[dim_z_*dim_y_*k+dim_y_*j+i]=v(i,j,k);
			}
		}
  	float *c = new float[dim_x()];
  	for(int k=0;k<dim_z();++k) {
		  for(int j=0;j<dim_y();++j) {
		  	for(int i=0;i<dim_x();++i)
		  		c[i]=(*this)(i,j,k);
		  	ConvertToInterpolationCoefficients(c,dim_x());
		  	for(int i=0;i<dim_x();++i) 
          data_[dim_z_*dim_y_*k+dim_y_*j+i]=c[i];
		  }
	  }
	  delete c;
	  c = new float[dim_y()];
  	for(int k=0;k<dim_z();++k) {
		  for(int i=0;i<dim_x();++i) {
		    for(int j=0;j<dim_y();++j) 
		  		c[j]=(*this)(i,j,k);
		  	ConvertToInterpolationCoefficients(c,dim_y());
		    for(int j=0;j<dim_y();++j) 
          data_[dim_z_*dim_y_*k+dim_y_*j+i]=c[j];
		  }
	  }
	  delete c;
	  c = new float[dim_z()];
		for(int j=0;j<dim_y();++j) {
		  for(int i=0;i<dim_x();++i) {
  	    for(int k=0;k<dim_z();++k) 
		  		c[k]=(*this)(i,j,k);
		  	ConvertToInterpolationCoefficients(c,dim_z());
  	    for(int k=0;k<dim_z();++k) 
          data_[dim_z_*dim_y_*k+dim_y_*j+i]=c[k];
		  }
	  }
	  delete c;
	}
	virtual double operator()(double x, double y, double z) const {
	  x=(dim_x_-1)*x;
	  y=(dim_y_-1)*y;
	  z=(dim_z_-1)*z;
		int ix=x;
		int iy=y;
		int iz=z;
		float dx=x-ix;
		float dy=y-iy;
		float dz=z-iz;
  	double result=0.0;
	  for (int zo=-1; zo <= 2; zo++)  //range [-1, 2]
	  {
	  	float b_z = bspline(dz-zo);
		  for (int yo=-1; yo <= 2; yo++)
		  {
		  	float b_y = bspline(dy-yo);
			  for (int xo=-1; xo <= 2; xo++)
			  {
			  	float b_x = bspline(dx-xo);
				  result   += (*this)(ix+xo,iy+yo,iz+zo)*b_x*b_y*b_z;
			  }
		  }
	  }
	  return result;
	}
	double operator()(int x, int y, int z) const {
   	x = x < 0 ? -x : (x >= dim_x() ? (2*dim_x()-x-2) : x);
   	y = y < 0 ? -y : (y >= dim_y() ? (2*dim_y()-y-2) : y);
   	z = z < 0 ? -z : (z >= dim_z() ? (2*dim_z()-z-2) : z);
	  return static_cast<double>(data_[dim_z_*dim_y_*z+dim_y_*y+x]);
	}
	virtual void load(const char * filename) {
	}
	virtual vec<3,double>
	grad(double x, double y, double z) const {
	  x=(dim_x_-1)*x;
	  y=(dim_y_-1)*y;
	  z=(dim_z_-1)*z;
		int ix=x;
		int iy=y;
		int iz=z;
		float dx=x-ix;
		float dy=y-iy;
		float dz=z-iz;
	  vec<3,double> result;
	  for (int zo=-1; zo <= 2; zo++)  //range [-1, 2]
	  {
		  for (int yo=-1; yo <= 2; yo++)
		  {
			  for (int xo=-1; xo <= 2; xo++)
			  {
				  float c=(*this)(ix+xo,iy+yo,iz+zo);
				  result[0] += c*bsplined(xo-dx)*bspline(yo-dy)*bspline(zo-dz);
				  result[1] += c*bspline(xo-dx)*bsplined(yo-dy)*bspline(zo-dz);
				  result[2] += c*bspline(xo-dx)*bspline(yo-dy)*bsplined(zo-dz);
			  }
		  }
	  }
    return result;
	}
	virtual ~tricubic_vol() {
	  delete [] data_;
	}
};


#endif // VOL_HPP

